#include "SC7I22_driver.h"
#include "I2C.h"
#include "delay.h"
#include "UART.h"


unsigned char imu_data[12];

void SL_SC7A20H_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char Txdata)
{
	if (sl_spi_iic == 0)
	{
		/* code */
	}
	else if (sl_spi_iic == 1)
	{
		EAXSFR();						 /* MOVX A,@DPTR/MOVX @DPTR,A指令的操作对象为扩展SFR(XSFR) */
		Start();						 // 发送起始命令
		SendData(0x32); // 发送设备地址+写命令
		RecvACK();
		SendData(reg); // 发送存储地址
		RecvACK();
		
		do
		{
			SendData(Txdata);
			RecvACK();
		} while (0);
		Stop();	  // 发送停止命令
		EAXRAM(); /* MOVX A,@DPTR/MOVX @DPTR,A指令的操作对象为扩展RAM(XRAM) */
	}
}
void SL_SC7A20H_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned char len, unsigned char *buf)
{
	if (sl_spi_iic == 0)
	{
		/* code */
	}
	else if (sl_spi_iic == 1)
	{
		EAXSFR();						 /* MOVX A,@DPTR/MOVX @DPTR,A指令的操作对象为扩展SFR(XSFR) */
		Start();						 // 发送起始命令
		SendData(0x32); // 发送设备地址+写命令
		RecvACK();
		SendData(reg); // 发送存储地址
		RecvACK();
		Start();							   // 发送起始命令
		SendData(0x33); // 发送设备地址+读命令
		RecvACK();
		do
		{
			*buf = RecvData();
			buf++;
			if (len != 1)
				SendACK(); // send ACK
		} while (--len);
		SendNAK(); // send no ACK
		Stop();	   // 发送停止命令
		EAXRAM();  /* MOVX A,@DPTR/MOVX @DPTR,A指令的操作对象为扩展RAM(XRAM) */
	}
}

void sc7122_init()
{
	// gyroStatus = GYRO_INITFAIL;	
	unsigned char temp;
	unsigned char i = 0;
	SL_SC7A20H_I2c_Spi_Write(1,0x4a,0xa5);		// 写入0xa5复位整个电路
	delay_ms(1000);
	SL_SC7A20H_I2c_Spi_Write(1, 0x7F, 0x00);
	// 建立通信
	do
	{
		temp = 0x00;
		SL_SC7A20H_I2c_Spi_Read(1, WHO_AM_I_REG, 1,&temp);
		if (temp == WHO_AM_I_VAL)
		{
			// gyroStatus = GYRO_INITOK;
			break;
		}

	} while (i++ < GYRO_VERIFY_ID_REPEAT_TIMES);

	SL_SC7A20H_I2c_Spi_Write(1, 0x7d, 0x0e); // 使能陀螺仪和加速度计		0x7D 要先写入 0x0E 并延时几毫秒之后再进行其他配置。
	delay_ms(100);
	SL_SC7A20H_I2c_Spi_Write(1, 0x30, 0x03); // 关闭内部上拉电阻
	delay_ms(1);	
	SL_SC7A20H_I2c_Spi_Write(1, 0x08, 0x80|0x00); // LPF  不使能低通滤波器
	delay_ms(1);
	// 量程设置
	SL_SC7A20H_I2c_Spi_Write(1, ACC_RANGE_REG, 0x00); //+-2g (00：+/- 2G； 01： +/- 4G； 10： +/- 8G； 11： +/- 16G)
	delay_ms(1);
	SL_SC7A20H_I2c_Spi_Write(1, GYRO_RANGE_REG, GYRO_RANGE_SEL); //+-2000dps
	delay_ms(1);
	// Odr设置
	SL_SC7A20H_I2c_Spi_Write(1, ACC_CONF, ACC_HIGH | ACC_AVE1 | ACC_400HZ); // 高性能模式，ODR400HZ，带宽与滤波模式:1.6KHZ/ODR
	delay_ms(1);
	SL_SC7A20H_I2c_Spi_Write(1, GYRO_CONF, GYRO_HIGH | GYRO_AVE1 | GYRO_400HZ | GYRO_NOISE); // GYRO,SAMPFRE,1AVE,noise
	delay_ms(1);
	// 更新设置
	SL_SC7A20H_I2c_Spi_Write(1, 0x05, 0x50);	
	delay_ms(1);
}

void get_acc_gyro_raw_SC7112(_st_SC7I22 * Data)
{
	SL_SC7A20H_I2c_Spi_Read(1, 0x0c, 12, &imu_data[0]);
	Data->ax = (imu_data[0] << 8) + imu_data[1];
	Data->ay = (imu_data[2] << 8) + imu_data[3];
	Data->az = (imu_data[4] << 8) + imu_data[5];

	Data->gx = (imu_data[6] << 8) + imu_data[7];
	Data->gy = (imu_data[8] << 8) + imu_data[9];
	Data->gz = (imu_data[10] << 8) + imu_data[11];
}
